﻿#pragma once

#include <xy/geom/bspline.h>

namespace xy
{

namespace geom
{

template <std::size_t N, bool Rat = false> class bspline_curve : public bspline<1, N, Rat>
{
    using vecd_t = vec<1, double>;       // 参数域向量
    using vecr_t = vec<N + Rat, double>; // (wP,w)
    using vec_t = vec<N, double>;        // P
    using bspline<1, N, Rat>::m_cpts;

  public:
    using bspline<1, N, Rat>::degree;
    using bspline<1, N, Rat>::knots;
    using bspline<1, N, Rat>::knot_num;
    using bspline<1, N, Rat>::set_poles;
    using bspline<1, N, Rat>::pole_num;
    using bspline<1, N, Rat>::weight;
    using bspline<1, N, Rat>::set_weight;
    using bspline<1, N, Rat>::bound;

    /**
     * @brief SplineCurve 构造函数
     *
     * @param[in] cpts
     * @param[in] knots
     * @param[in] degree
     * @param[in] normalize
     */
    bspline_curve(std::vector<vecr_t> &&cpts, std::vector<double> &&knots, std::size_t degree, bool normalize = true)
        : bspline<1, N, Rat>(math::ndarray<1, vecr_t>{{std::move(cpts)}}, {std::move(knots)}, {{degree}}, normalize)
    {
    }

    /**
     * @brief 计算曲线上点的位置
     *
     * @param[in] param
     * @return vec_t
     */
    vec_t d0(const vecd_t &param) const override
    {
        auto p = curve_point(m_cpts.data(), knots().data(), knot_num(), degree(), param[0]);
        if constexpr (Rat)
            return homogeneous_to_cartesian(p);
        else
            return p;
    }

    /**
     * @brief 计算曲线上点的位置和导数
     *
     * @param[in] param
     * @return std::array<vec_t, 2>
     */
    std::array<vec_t, 2> d1(const vecd_t &param) const override
    {
        auto t = param[0];
        if constexpr (Rat)
        {
            auto ders = rat_curve_deriv(m_cpts.data(), knots().data(), knots().size(), degree(), 1, t);
            return {ders[0], ders[1]};
        }
        else
        {
            auto ders = curve_deriv_alg1(m_cpts.data(), knots().data(), knots().size(), degree(), 1, t);
            return {ders[0], ders[1]};
        }
    }

    /**
     * @brief 计算曲线上点的位置、导数和二阶导数
     *
     * @param[in] param
     * @return std::array<vec_t, 3>
     */
    std::array<vec_t, 3> d2(const vecd_t &param) const override
    {
        auto t = param[0];
        if constexpr (Rat)
        {
            auto ders = rat_curve_deriv(m_cpts.data(), knots().data(), knots().size(), degree(), 2, t);
            return {ders[0], ders[1], ders[2]};
        }
        else
        {
            auto ders = curve_deriv_alg1(m_cpts.data(), knots().data(), knots().size(), degree(), 2, t);
            return {ders[0], ders[1], ders[2]};
        }
    }

    /**
     * @brief 将 t 处插入 r 次节点
     *
     * @param[in] t
     * @param[in] r
     * @return true
     * @return false
     */
    bool insert_knot(double t, std::size_t r = 1)
    {
        auto &m_knots = knots();
        auto span = find_span(m_knots.data(), m_knots.size(), degree(), t);
        auto s = find_mult_span(m_knots.data(), span, degree(), t);
        if (s + r > degree())
            return false;

        std::vector<vecr_t> Q(pole_num() + r);
        std::vector<double> U(knot_num() + r);
        curve_knot_ins(m_cpts.data(), m_knots.data(), m_knots.size(), degree(), t, span, s, r, Q.data(), U.data());

        m_knots = std::move(U);
        set_poles(std::move(Q));

        return true;
    }

    /**
     * @brief 细化曲线
     *
     * @param[in] x
     * @return true
     * @return false
     */
    bool refine_knot(std::vector<double> &&x)
    {
        if (x.empty())
            return false;

        auto &m_knots = knots();
        std::vector<vecr_t> Q(x.size() + pole_num());
        std::vector<double> U(x.size() + knot_num());

        curve_knot_refine(m_cpts.data(), m_knots.data(), m_knots.size(), degree(), x.data(), x.size(), Q.data(),
                          U.data());

        m_knots = std::move(U);
        set_poles(std::move(Q));

        return true;
    }

    /**
     * @brief 移除 t 处的 r 次节点
     *
     * @param[in] t
     * @param[in] r
     * @return int
     */
    int remove_knot(double t, std::size_t r = 1)
    {
        auto &m_knots = knots();
        auto span = find_span(m_knots.data(), m_knots.size(), degree(), t);
        auto s = find_mult_span(m_knots.data(), span, degree(), t);
        auto num =
            curve_knot_remove<vecr_t, Rat>(m_cpts.data(), m_knots.data(), m_knots.size(), degree(), t, span, s, r);
        m_cpts.reshape(m_cpts.size() - num);
        m_knots.resize(m_knots.size() - num);
        return num;
    }

    /**
     * @brief 升次到 n 次 BSpline 曲线
     *
     * @param[in] n
     * @return true
     * @return false
     */
    bool elevate(std::size_t n)
    {
        if (n <= degree())
            return false;

        auto &m_knots = knots();
        auto &m_degree = degree();
        std::vector<double> U(m_knots.size() * n);
        std::vector<vecr_t> Q(U.size() - n - 1);

        auto num =
            curve_elevate(m_cpts.data(), m_knots.data(), m_knots.size(), degree(), n - degree(), Q.data(), U.data());
        U.resize(num);
        Q.resize(num - n - 1);

        // 注意顺序，先修改 m_degree，再修改 m_cpts
        m_knots = std::move(U);
        m_degree = n;
        set_poles(std::move(Q));

        return true;
    }

    /**
     * @brief 降次 1 次
     *
     * @param[in] tol
     * @return true
     * @return false
     */
    bool reduce(double tol)
    {
        if (degree() <= 1)
            return false;

        auto &m_knots = knots();
        auto &m_degree = degree();
        std::vector<double> U(m_knots.size());
        std::vector<vecr_t> Q(U.size() - degree() - 2);
        auto num = curve_reduce(m_cpts.data(), m_knots.data(), m_knots.size(), degree(), Q.data(), U.data(), tol);
        if (num == -1)
            return false;

        U.resize(num);
        Q.resize(num - degree() - 2);

        // 注意顺序，先修改 m_degree，再修改 m_cpts
        m_knots = std::move(U);
        m_degree--;
        set_poles(std::move(Q));

        return true;
    }

    /**
     * @brief 线性重参数化到 [c,d]
     *
     * @param[in] c
     * @param[in] d
     */
    void reparam(double c, double d)
    {
        auto &m_knots = knots();
        auto c0 = m_knots[0];
        auto d0 = m_knots[m_knots.size() - 1];
        for (auto &k : m_knots)
            k = (k - c0) / (d0 - c0) * (d - c) + c;
    }

    /**
     * @brief 有理重参数化 s = g(u) = (alpha * u + beta) / (gamma * u + delta)
     * u \in [a,b] -> s \in [c,d]
     *
     * @param[in] alpha
     * @param[in] beta
     * @param[in] gamma
     * @param[in] delta
     * @return true
     * @return false
     */
    bool reparam(double alpha, double beta, double gamma, double delta)
    {
        static_assert(Rat, "Rational curve required.");
        if (alpha * delta - beta * gamma <= 0)
            return false;

        auto &m_knots = knots();
        auto mu = [gamma, delta](double u) { return gamma * u + delta; };
        double factor = 1.0;
        for (std::size_t j = 1; j < degree() + 1; ++j)
            factor *= mu(m_knots[j]);

        for (std::size_t i = 0; i < pole_num(); ++i)
        {
            set_weight(weight(i) / factor, i);
            factor *= mu(m_knots[degree() + i + 1]) / mu(m_knots[i + 1]);
        }

        std::transform(m_knots.begin(), m_knots.end(), m_knots.begin(),
                       [alpha, beta, gamma, delta](double u) { return (alpha * u + beta) / (gamma * u + delta); });

        return true;
    }

    /**
     * @brief 有理重参数化到 [c,d]，使得 w0 = wn
     *
     * @param[in] c
     * @param[in] d
     * @return true
     * @return false
     */
    bool reparam_to_same_weights(double c, double d)
    {
        auto w0 = weight(0);
        auto wn = weight(pole_num() - 1);
        if (w0 == wn)
            return true;

        auto [a, b] = bound();
        auto p = 1.0 / degree();
        w0 = std::pow(w0, p);
        wn = std::pow(wn, p);
        auto alpha = wn * d - w0 * c;
        auto beta = w0 * b * c - wn * a * d;
        auto gamma = wn - w0;
        auto delta = w0 * b - wn * a;
        return reparam(alpha, beta, gamma, delta);
    }

    /**
     * @brief 反转曲线
     *
     * @return * void
     */
    void reverse()
    {
        auto &m_knots = knots();
        auto c = m_knots[0] + m_knots[m_knots.size() - 1];
        for (auto &k : m_knots)
            k = c - k;
        std::reverse(m_knots.begin(), m_knots.end());
        std::reverse(m_cpts.flatten().begin(), m_cpts.flatten().end());
    }

    /**
     * @brief 创建一条线性 BSpline 曲线
     *
     * @param[in] p0 起点
     * @param[in] p1 终点
     * @param[in] k0 起始参数
     * @param[in] k1 终止参数
     * @return std::shared_ptr<bspline_curve>
     */
    static std::shared_ptr<bspline_curve> linear(const vec_t &p0, const vec_t &p1, double k0 = 0.0, double k1 = 1.0)
    {
        std::vector<double> knots = {k0, k0, k1, k1};
        if constexpr (Rat)
        {
            std::vector<vecr_t> cpts = {cartesian_to_homogeneous(p0), cartesian_to_homogeneous(p1)};
            return std::make_shared<bspline_curve>(std::move(cpts), std::move(knots), 1);
        }
        else
        {
            std::vector<vecr_t> cpts = {p0, p1};
            return std::make_shared<bspline_curve>(std::move(cpts), std::move(knots), 1);
        }
    }

    /**
     * @brief 节点插入使得两条曲线具有相同的节点向量和次数
     *
     * @param[in] c1
     * @param[in] c2
     */
    static void compat(std::shared_ptr<bspline_curve> c1, std::shared_ptr<bspline_curve> c2)
    {
        std::size_t d = std::max(c1->degree(), c2->degree());

        c1->elevate(d);
        c2->elevate(d);

        // 对齐两个曲线的第一个和最后一个节点
        auto [first, last] = c1->bound();
        c2->reparam(first, last);

        auto knots1 = c1->knots();
        auto knots2 = c2->knots();

        std::vector<double> ins1;
        std::vector<double> ins2;

        std::size_t i = d + 1, j = d + 1;
        while (i < knots1.size() - d - 1 || j < knots2.size() - d - 1)
        {
            auto d = knots1[i] - knots2[j];
            if (std::abs(d) < math::numeric::half)
            {
                // 节点重合，跳过
            }
            else if (d < math::numeric::half)
            {
                ins2.push_back(knots1[i]);
                knots2.insert(knots2.begin() + j, knots1[i]);
            }
            else if (-d < math::numeric::half)
            {
                ins1.push_back(knots2[j]);
                knots1.insert(knots1.begin() + i, knots2[j]);
            }

            if (i < knots1.size() - d - 1)
                i++;
            if (j < knots2.size() - d - 1)
                j++;
        }

        c1->refine_knot(std::move(ins1));
        c2->refine_knot(std::move(ins2));
    }
};

template <std::size_t N> struct bspline_curve_qr : public bspline_curve<N, true>
{
    using vecr_t = vec<N + 1, double>; // (wP,w)
    using vec_t = vec<N, double>;      // P

  public:
    bspline_curve_qr(bspline_curve_qr &&) = delete;

    /**
     * @brief Construct a new bspline_curve_qr object
     *
     * @param[in] cpts
     * @param[in] knots
     * @param[in] normalize
     */
    bspline_curve_qr(std::vector<vecr_t> &&cpts, std::vector<double> &&knots, bool normalize = true)
        : bspline_curve<N, true>(std::move(cpts), std::move(knots), 2, normalize)
    {
    }

    /**
     * @brief 创建一个圆弧 BSpline 曲线
     *
     * @param[in] O 圆心
     * @param[in] X 主轴
     * @param[in] Y 副轴
     * @param[in] r 半径
     * @param[in] thetaStart 起始角度
     * @param[in] thetaEnd 终止角度
     * @return std::shared_ptr<bspline_curve_qr>
     */
    static std::shared_ptr<bspline_curve_qr> make_circle(const vec_t &O, const vec_t &X, const vec_t &Y, double r,
                                                         double thetaStart, double thetaEnd)
    {
        thetaStart = math::degrees_to_radians(thetaStart);
        thetaEnd = math::degrees_to_radians(thetaEnd);
        if (thetaEnd < thetaStart)
            thetaEnd += 2 * math::numeric::pi;

        auto theta = thetaEnd - thetaStart;
        std::size_t arcNum = 4;
        if (theta <= math::numeric::pi / 2)
            arcNum = 1;
        else if (theta <= math::numeric::pi)
            arcNum = 2;
        else if (theta <= 3 * math::numeric::pi / 2)
            arcNum = 3;

        auto dTheta = theta / arcNum;
        auto angle = thetaStart;
        auto w1 = std::cos(dTheta / 2);
        auto P0 = O + X * r * std::cos(thetaStart) + Y * r * std::sin(thetaStart);
        auto T0 = X * -sin(thetaStart) + Y * cos(thetaStart);

        std::vector<vecr_t> cpts(arcNum * 2 + 1);
        cpts[0] = cartesian_to_homogeneous(P0);
        for (std::size_t i = 0; i < arcNum; ++i)
        {
            angle += dTheta;
            auto P2 = O + X * r * std::cos(angle) + Y * r * std::sin(angle);
            auto T2 = X * -sin(angle) + Y * cos(angle);

            auto option = intersect_line_line(P0, T0, P2, T2);
            auto [alpha0, alpha2] = option.value();

            cpts[i * 2 + 1] = cartesian_to_homogeneous(P0 + T0 * alpha0) * w1;
            cpts[i * 2 + 2] = cartesian_to_homogeneous(P2);

            P0 = P2;
            T0 = T2;
        }

        auto j = 2 * arcNum + 1;
        std::vector<double> knots(j + 3);
        for (std::size_t i = 0; i < 3; i++)
        {
            knots[i] = 0;
            knots[j + i] = 1;
        }

        switch (arcNum)
        {
        case 1:
            break;
        case 2:
            knots[3] = knots[4] = 0.5;
            break;
        case 3:
            knots[3] = knots[4] = 1 / 3.0;
            knots[5] = knots[6] = 2 / 3.0;
            break;
        case 4:
            knots[3] = knots[4] = 1 / 4.0;
            knots[5] = knots[6] = 1 / 2.0;
            knots[7] = knots[8] = 3 / 4.0;
            break;
        default:
            break;
        }

        return std::make_shared<bspline_curve_qr>(std::move(cpts), std::move(knots), false);
    }
};

} // namespace geom

} // namespace xy